#include "Rotate.h"

void Rotate::start(const cv::Mat &image ,const Bbox &finalBbox, cv::Mat& rotRes) {
	double dst_landmark[10] = {
		38.2946, 73.5318, 55.0252, 41.5493, 70.7299,
		51.6963, 51.5014, 71.7366, 92.3655, 92.2041 };
	std::vector<cv::Point2f>coord5points;
	std::vector<cv::Point2f>facePointsByMtcnn;

	for (int i = 0; i < 5; i++) 
		coord5points.push_back(cv::Point2f(dst_landmark[i], dst_landmark[i + 5]));
	for (int j = 0; j < 5; j = j + 1)
		facePointsByMtcnn.push_back(cvPoint(finalBbox.ppoint[j], finalBbox.ppoint[j + 5]));

	cv::Mat warp_mat = cv::estimateRigidTransform(facePointsByMtcnn, coord5points, false);
	if (warp_mat.empty())
		getsrc_roi(facePointsByMtcnn, coord5points, warp_mat);
	warp_mat.convertTo(warp_mat, CV_32FC1);
	rotRes = cv::Mat::zeros(112, 112, image.type());
	warpAffine(image, rotRes, warp_mat, rotRes.size());
}

void Rotate::getsrc_roi(std::vector<cv::Point2f> x0, std::vector<cv::Point2f> dst, cv::Mat& roi) {
	int size = dst.size();
	cv::Mat A = cv::Mat::zeros(size * 2, 4, CV_32FC1);
	cv::Mat B = cv::Mat::zeros(size * 2, 1, CV_32FC1);

	for (int i = 0; i < size; i++)
	{
		A.at<float>(i << 1, 0) = x0[i].x;// roi_dst[i].x;
		A.at<float>(i << 1, 1) = -x0[i].y;
		A.at<float>(i << 1, 2) = 1;
		A.at<float>(i << 1, 3) = 0;
		A.at<float>(i << 1 | 1, 0) = x0[i].y;
		A.at<float>(i << 1 | 1, 1) = x0[i].x;
		A.at<float>(i << 1 | 1, 2) = 0;
		A.at<float>(i << 1 | 1, 3) = 1;
		B.at<float>(i << 1) = dst[i].x;
		B.at<float>(i << 1 | 1) = dst[i].y;
	}

	roi = cv::Mat::zeros(2, 3, A.type());
	cv::Mat AT = A.t();
	cv::Mat ATA = A.t() * A;
	cv::Mat R = ATA.inv() * AT * B;

	roi.at<float>(0, 0) = R.at<float>(0, 0);
	roi.at<float>(0, 1) = -R.at<float>(1, 0);
	roi.at<float>(0, 2) = R.at<float>(2, 0);
	roi.at<float>(1, 0) = R.at<float>(1, 0);
	roi.at<float>(1, 1) = R.at<float>(0, 0);
	roi.at<float>(1, 2) = R.at<float>(3, 0);
}
